/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of
*     its contributors may be used to endorse or promote products
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <iostream>
#include <iomanip>

#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>

// ---------------------------------------------------------------------------
int main(int argc, char **argv)
{
	ros::init(argc, argv, "sim_clock");
	ros::NodeHandle node;

	ROS_INFO_STREAM("Setting ROS param /use_sim_time to true.");
	ros::param::set("/use_sim_time", true);

	bool time_free_running;
	if (!ros::param::get("/time_free_running", time_free_running)) {
		time_free_running = false;
		ROS_INFO_STREAM("Param /time_free_running not set, using default: "
				<< (time_free_running ? "true" : "false"));
	}

	double time_scale = 1.0;
	if (!ros::param::getCached("/time_scale", time_scale)) {
		ROS_INFO_STREAM("Param /time_scale not set, using default: " << time_scale);
		ros::param::set("/time_scale", time_scale);
	}

	double clock_publish_rate = 100.0;
	if (!ros::param::get("/clock_publish_rate", clock_publish_rate)) {
		ROS_INFO_STREAM("Param /clock_publish_rate not set, using default: " << clock_publish_rate);
	}
	ros::Duration clock_publish_period(1.0 / clock_publish_rate);

	ros::WallTime real_start_time = ros::WallTime::now();
	ros::Time sim_start_time;
	int start_sec;
	int start_nsec;
	if (ros::param::get("/time_start_sec", start_sec) && ros::param::get("/time_start_nsec", start_nsec)) {
		sim_start_time.sec = start_sec;
		sim_start_time.nsec = start_nsec;
		ROS_INFO_STREAM("Start time (from param) : " << sim_start_time.sec << "."
				<< std::setw(9) << std::setfill('0') << sim_start_time.nsec);
	}
	else {
		sim_start_time.sec = real_start_time.sec;
		sim_start_time.nsec = real_start_time.nsec;
		ROS_INFO_STREAM("Start time (from wall)  : " << sim_start_time.sec << "."
				<< std::setw(9) << std::setfill('0') << sim_start_time.nsec);
	}

	if (time_free_running) {
		ROS_INFO_STREAM("Time simulation mode    : free running, fixed increment");
	}
	else {
		ROS_INFO_STREAM("Time simulation mode    : linked to wall clock");
	}
	ROS_INFO_STREAM  ("Current time scale      : " << time_scale);
	ROS_INFO_STREAM  ("Publishing at wall rate : " << clock_publish_rate << " Hz");

	// Variables relating to real, wall time.
	ros::WallTime real_time;
	ros::WallDuration real_duration;
	int usleep_period = (int)(1000000.0 / clock_publish_rate);

	// Variables relating to simulation time.
	ros::Duration sim_duration(clock_publish_period.toSec() * time_scale);

	// Variables to allow us to check the scale factor approx every 100 ms,
	// without doing expensive time checks.
	int check_scale_every = clock_publish_rate * 0.1;
	int check_scale_count = 1;
	if (check_scale_every <= 0) check_scale_every = 1;

	// Create publisher and clock msg.
	ros::Publisher clock_pub = node.advertise<rosgraph_msgs::Clock>("/clock", 50);
	rosgraph_msgs::Clock clock_msg;
	// Set the first time in the msg.
	clock_msg.clock = sim_start_time;

	if (time_free_running) {
		// In this simple mode, the simulation clock runs free, and we
		// just increment it with a fixed value each time round the loop.

		while (true) {
			clock_msg.clock += sim_duration;
			clock_pub.publish(clock_msg);

			check_scale_count--;
			if (check_scale_count <= 0) {
				check_scale_count = check_scale_every;

				// Check ros::ok() here rather than outer loop, to avoid wasting cycles.
				if (!ros::ok()) break;

				double new_time_scale;
				if (ros::param::getCached("/time_scale", new_time_scale) && new_time_scale != time_scale) {
					time_scale = new_time_scale;
					sim_duration.fromSec(clock_publish_period.toSec() * time_scale);
					ROS_INFO_STREAM("Param /time_scale changed to " << new_time_scale
							<< ", new fixed time increment " << sim_duration.toSec());
				}
			}

			usleep(usleep_period);
		}
	}
	else {
		// In this mode, simulation time is strictly linked to wall time
		// by time_scale.

		while (true)
		{
			real_time = ros::WallTime::now();
			real_duration = real_time - real_start_time;
			sim_duration.fromSec(real_duration.toSec() * time_scale);

			clock_msg.clock = sim_start_time + sim_duration;
			clock_pub.publish(clock_msg);

			check_scale_count--;
			if (check_scale_count <= 0) {
				check_scale_count = check_scale_every;

				// Check ros::ok() here rather than outer loop, to avoid wasting cycles.
				if (!ros::ok()) break;

				double new_time_scale;
				if (ros::param::getCached("/time_scale", new_time_scale) && new_time_scale != time_scale) {
					real_start_time += real_duration;
					sim_start_time += sim_duration;
					time_scale = new_time_scale;
					ROS_INFO_STREAM("Param /time_scale changed to " << new_time_scale);
				}
			}

			usleep(usleep_period);
		}
	}

	// Can't use ROS_INFO here because ROS is shutting down.

	std::cout << "Shutting down." << std::endl;

	std::cout << "Setting ROS param use_time_time to false." << std::endl;
	ros::param::set("/use_sim_time", false);

	return 0;
}
